#include <string.h>
#include <stdio.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "freertos/event_groups.h"
#include "esp_wifi.h"
#include "esp_event.h"
#include "esp_log.h"
#include "nvs_flash.h"
#include "esp_netif.h"
#include "driver/adc.h"
#include "esp_system.h"
#include "lwip/sockets.h"
#include "lwip/netdb.h"

// ======================= 配置区 ==========================
// 目标AP（接收端作为AP，控制端以STA连接）
#define WIFI_SSID               "esp32_motor_ap"
#define WIFI_PASSWORD           "12345678"

// UDP 发送目标
#define RECEIVER_IP             "192.168.4.1"
#define UDP_PORT                4210

// 发送频率（20Hz -> 50ms）
#define SEND_PERIOD_MS          50

// 控制输入：使用 ADC1 通道6（GPIO34）
#define CONTROLLER_ADC_CHANNEL  ADC1_CHANNEL_6   // GPIO34
#define ADC_WIDTH_CONFIG        ADC_WIDTH_BIT_12 // 12位，0-4095
#define ADC_ATTEN_CONFIG        ADC_ATTEN_DB_11  // 量程适配 0-3.3V

// 日志TAG
static const char *TAG = "controller_sender";

// WiFi 事件组与事件位
static EventGroupHandle_t s_wifi_event_group;
static const int WIFI_CONNECTED_BIT = BIT0;

// =================== WiFi 事件回调 =======================
static void wifi_event_handler(void *arg, esp_event_base_t event_base, int32_t event_id, void *event_data) {
    if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_START) {
        esp_wifi_connect();
    } else if (event_base == WIFI_EVENT && event_id == WIFI_EVENT_STA_DISCONNECTED) {
        // 断开后重连
        ESP_LOGW(TAG, "WiFi 断开，尝试重连...");
        esp_wifi_connect();
        xEventGroupClearBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    } else if (event_base == IP_EVENT && event_id == IP_EVENT_STA_GOT_IP) {
        ip_event_got_ip_t *event = (ip_event_got_ip_t *)event_data;
        ESP_LOGI(TAG, "获取到IP: " IPSTR, IP2STR(&event->ip_info.ip));
        xEventGroupSetBits(s_wifi_event_group, WIFI_CONNECTED_BIT);
    }
}

// =================== WiFi 初始化（STA） ====================
static void init_wifi_sta(void) {
    // 初始化网络栈
    ESP_ERROR_CHECK(esp_netif_init());
    ESP_ERROR_CHECK(esp_event_loop_create_default());
    esp_netif_create_default_wifi_sta();

    wifi_init_config_t cfg = WIFI_INIT_CONFIG_DEFAULT();
    ESP_ERROR_CHECK(esp_wifi_init(&cfg));

    // 注册事件回调
    ESP_ERROR_CHECK(esp_event_handler_instance_register(WIFI_EVENT, ESP_EVENT_ANY_ID, &wifi_event_handler, NULL, NULL));
    ESP_ERROR_CHECK(esp_event_handler_instance_register(IP_EVENT, IP_EVENT_STA_GOT_IP, &wifi_event_handler, NULL, NULL));

    // 配置为 STA 模式并设置目标 AP 信息
    wifi_config_t wifi_config = {0};
    strncpy((char *)wifi_config.sta.ssid, WIFI_SSID, sizeof(wifi_config.sta.ssid) - 1);
    strncpy((char *)wifi_config.sta.password, WIFI_PASSWORD, sizeof(wifi_config.sta.password) - 1);
    wifi_config.sta.threshold.authmode = WIFI_AUTH_WPA2_PSK;
    wifi_config.sta.pmf_cfg.capable = true;
    wifi_config.sta.pmf_cfg.required = false;

    ESP_ERROR_CHECK(esp_wifi_set_mode(WIFI_MODE_STA));
    ESP_ERROR_CHECK(esp_wifi_set_config(WIFI_IF_STA, &wifi_config));
    ESP_ERROR_CHECK(esp_wifi_start());
}

// =================== ADC 初始化与读取 =====================
static void init_adc_controller_input(void) {
    // 配置 ADC1 宽度与通道衰减
    ESP_ERROR_CHECK(adc1_config_width(ADC_WIDTH_CONFIG));
    ESP_ERROR_CHECK(adc1_config_channel_atten(CONTROLLER_ADC_CHANNEL, ADC_ATTEN_CONFIG));
}

// 读取控制器输入并转换为方向与速度
// 方向：-1 左，0 静止，1 右
// 速度：0-255
static void read_controller_input(int *out_direction, int *out_speed) {
    // 读取原始 ADC 值（0-4095）
    int adc_raw = adc1_get_raw(CONTROLLER_ADC_CHANNEL);

    // 将 Python 版本的 16位逻辑映射到 12位：
    // center = 32768 -> 2048
    // 高阈值 34000 -> 按比例映射
    // 低阈值 31500 -> 按比例映射
    const int adc_max = 4095;
    const int center = 2048;
    const int thr_high = center + (int)((34000 - 32768) * (float)adc_max / 65535.0f);
    const int thr_low  = (int)(31500 * (float)adc_max / 65535.0f);

    int direction = 0;
    int speed = 0;

    if (adc_raw > thr_high) {
        direction = 1;
        int num = (adc_raw - center) * 255;
        int den = (adc_max - center);
        speed = (den > 0) ? (num / den) : 0;
        if (speed > 255) speed = 255;
        if (speed < 0) speed = 0;
    } else if (adc_raw < thr_low) {
        direction = -1;
        int num = (thr_low - adc_raw) * 255;
        int den = (thr_low > 0) ? thr_low : 1;
        speed = num / den;
        if (speed > 255) speed = 255;
        if (speed < 0) speed = 0;
    } else {
        direction = 0;
        speed = 0;
    }

    *out_direction = direction;
    *out_speed = speed;
}

// =================== UDP 发送函数 ========================
static int open_udp_socket(struct sockaddr_in *out_addr) {
    int sock = socket(AF_INET, SOCK_DGRAM, IPPROTO_IP);
    if (sock < 0) {
        ESP_LOGE(TAG, "创建 UDP socket 失败: errno=%d", errno);
        return -1;
    }

    memset(out_addr, 0, sizeof(struct sockaddr_in));
    out_addr->sin_family = AF_INET;
    out_addr->sin_port = htons(UDP_PORT);
    out_addr->sin_addr.s_addr = inet_addr(RECEIVER_IP);

    return sock;
}

static void send_control_data(int sock, const struct sockaddr_in *addr, int direction, int speed) {
    char payload[32];
    // 与 Python 端保持一致的文本网格："direction,speed"
    int len = snprintf(payload, sizeof(payload), "%d,%d", direction, speed);
    if (len <= 0) return;

    int ret = sendto(sock, payload, (size_t)len, 0, (const struct sockaddr *)addr, sizeof(struct sockaddr_in));
    if (ret < 0) {
        ESP_LOGE(TAG, "发送 UDP 数据失败: errno=%d", errno);
    } else {
        ESP_LOGI(TAG, "发送数据: 方向=%d, 速度=%d", direction, speed);
    }
}

// =================== 主发送任务 ==========================
static void controller_task(void *arg) {
    // 等待 WiFi 连接
    xEventGroupWaitBits(s_wifi_event_group, WIFI_CONNECTED_BIT, pdFALSE, pdTRUE, portMAX_DELAY);

    // 打开 UDP socket
    struct sockaddr_in dest_addr;
    int sock = open_udp_socket(&dest_addr);
    if (sock < 0) {
        ESP_LOGE(TAG, "无法创建 UDP socket，任务退出");
        vTaskDelete(NULL);
        return;
    }

    // 周期性读取与发送
    while (1) {
        int direction = 0;
        int speed = 0;
        read_controller_input(&direction, &speed);
        send_control_data(sock, &dest_addr, direction, speed);
        vTaskDelay(pdMS_TO_TICKS(SEND_PERIOD_MS));
    }
}

// =================== app_main 入口 =======================
void app_main(void) {
    // 初始化 NVS（WiFi 必需）
    esp_err_t ret = nvs_flash_init();
    if (ret == ESP_ERR_NVS_NO_FREE_PAGES || ret == ESP_ERR_NVS_NEW_VERSION_FOUND) {
        ESP_ERROR_CHECK(nvs_flash_erase());
        ret = nvs_flash_init();
    }
    ESP_ERROR_CHECK(ret);

    // 创建事件组
    s_wifi_event_group = xEventGroupCreate();

    // 初始化 WiFi STA
    init_wifi_sta();

    // 初始化 ADC 控制输入
    init_adc_controller_input();

    // 创建发送任务
    xTaskCreate(controller_task, "controller_task", 4096, NULL, 5, NULL);
} 